from machine import SPI, Pin
import utime
import st7789
import urandom
import math
from ulab import numpy as np

class Turtle:
    def __init__(self , display , x=120 , y=120 , backgroud_clr=st7789.color565(0, 0, 0)):
        self.backgroud_clr=backgroud_clr
        self.display=display
        self.zero_x=x
        self.zero_y=y
        self.last_x=x
        self.last_y=y
        self.now_x=x
        self.now_y=y
        self.pen=True
        self.turtle=False
        self.degree=90
        self.size=1
        self.pclr=st7789.color565(255, 255, 255)
        self.tclr=st7789.color565(0, 0, 0)
        self.display.init()
        self.display.fill(backgroud_clr)

        self.fill_clr=st7789.color565(255, 255, 255)
        self.fill_points=[]
        self.beg_fill=False
        self.fill_circle=False

        self.m="standard"


    def goto(self,x,y):
        if(self.turtle):
            self.drawturtle(self.degree,self.backgroud_clr)
        self.last_x=self.now_x
        self.last_y=self.now_y
        self.now_x=x+self.zero_x
        self.now_y=y+self.zero_y
        if(self.pen):
            self.drawline()
        if(self.turtle):
            self.drawturtle(self.degree,self.tclr)
        self.last_x=self.now_x
        self.last_y=self.now_y
        if(self.beg_fill):
            self.fill_points.append([self.now_x,self.now_y])
        return True

    def dot(self,d):
        if(self.pen):
            self.display.fill_circle(self.now_x,self.now_y,int(d/2),self.pclr)
        return True

    def penup(self):
        self.pen=False
        return True

    def pendown(self):
        self.pen=True
        return True

    def setheading(self,degree):
        if(self.turtle):
            self.drawturtle(self.degree,self.backgroud_clr)

        self.degree=(360-degree+90)%360
        if(self.turtle):
            self.drawturtle(self.degree,self.tclr)
        return True

    def right(self,degree):
        self.degree=(self.degree-(degree)%360+360)%360
        return True

    def left(self,degree):
        self.degree=(self.degree+(degree)%360+360)%360
        return True

    def drawline_direct_a(self,x1,y1,x2,y2,color,size):
        q=np.sqrt((x2-x1)**2+(y2-y1)**2)
        # print(q)
        if(q!=0):
            ux=(x2-x1)/q
            uy=(y2-y1)/q
            for l in np.arange(0,q,0.5):
                px=x1+l*ux
                py=y1+l*uy
                self.display.fill_circle(int(px), int(py), size, color)
        return True

    def drawline_direct_b(self,x1,y1,x2,y2,color,size):
        self.display.fill_circle(x2,y2,int(size/2),color)
        self.display.line(x1, y1, x2, y2, color)
        self.display.fill_circle(x1,y1,int(size/2),color)
        lens2=((y2-y1) ** 2)+((x2-x1) ** 2)
        lens=((lens2) **0.5)
        r=size/2
        if(lens!=0):
            qy=math.floor(r*(x2-x1)/lens)
            qx=math.floor(r*(y2-y1)/lens)
            
            self.display.line((x1-qx), (y1+qy), (x2-qx), (y2+qy), color)
            self.display.line((x1+qx), (y1-qy), (x2+qx), (y2-qy), color)
            
            nx1=x2-qx
            ny1=y2+qy
            nx2=x2+qx
            ny2=y2-qy

            lx1=x1-qx
            ly1=y1+qy
            lx2=x1+qx
            ly2=y1-qy
            
            l1=[]
            l2=[]
            q=np.sqrt((nx2-nx1)**2+(ny2-ny1)**2)
            if(q!=0):
                ux=(nx2-nx1)/q
                uy=(ny2-ny1)/q
                for l in np.arange(0,q,0.5):
                    px=nx1+l*ux
                    py=ny1+l*uy
                    l1.append([int(px),int(py)])
            if(q!=0):
                q=np.sqrt((lx2-lx1)**2+(ly2-ly1)**2)
                ux=(lx2-lx1)/q
                uy=(ly2-ly1)/q
                for l in np.arange(0,q,0.5):
                    px=lx1+l*ux
                    py=ly1+l*uy
                    l2.append([int(px),int(py)])
                for i in range(len(l1)):
                    self.display.line(l1[i][0], l1[i][1], l2[i][0], l2[i][1], color)
        return True

    def drawline(self):
        q=np.sqrt((self.now_x-self.last_x)**2+(self.now_y-self.last_y)**2)
        if(q==0):
            return
        ux=(self.now_x-self.last_x)/q
        uy=(self.now_y-self.last_y)/q
        for l in np.arange(0,q,0.5):
            px=self.last_x+l*ux
            py=self.last_y+l*uy
            self.display.fill_circle(int(px),int(py),int(self.size/2),self.pclr)
        return True
        
    def drawturtle(self,degree,color):
        if(self.turtle or color==self.backgroud_clr):
            tsize=13
            temp_x=int(math.sin(degree*math.pi/180)*tsize)
            temp_y=int(math.cos(degree*math.pi/180)*tsize)
            print(degree)
            print((degree+90)%360)
            temp_x1=int(math.sin((degree+90)%360*math.pi/180)*((tsize/2)-2))
            temp_y1=int(math.cos((degree+90)%360*math.pi/180)*((tsize/2)-2))
            temp_x2=int(math.sin((degree+270)%360*math.pi/180)*((tsize/2)-2))
            temp_y2=int(math.cos((degree+270)%360*math.pi/180)*((tsize/2)-2))
            ax=temp_x+self.now_x
            ay=temp_y+self.now_y
            bx=temp_x1+self.now_x
            by=temp_y1+self.now_y
            cx=temp_x2+self.now_x
            cy=temp_y2+self.now_y
            self.display.line(bx, by, ax, ay, color)
            self.display.line(cx, cy, bx, by, color)
            self.display.line(ax, ay, cx, cy, color)

            q=np.sqrt((bx-cx)**2+(by-cy)**2)
            ux=(bx-cx)/q
            uy=(by-cy)/q
            for l in np.arange(0,q,0.5):
                px=cx+l*ux
                py=cy+l*uy
                self.display.line(int(px), int(py), int(ax), int(ay), color)
        return True


    def forward(self,distance):
        if(self.turtle):
            self.drawturtle(self.degree,self.backgroud_clr)
        temp_x=int(math.sin(self.degree*math.pi/180)*distance)
        temp_y=int(math.cos(self.degree*math.pi/180)*distance)

        self.now_x=temp_x+self.last_x
        self.now_y=temp_y+self.last_y

        if(self.pen):
            self.drawline()
        self.last_x=self.now_x
        self.last_y=self.now_y
        if(self.turtle):
            self.drawturtle(self.degree,self.tclr)
        if(self.beg_fill):
            self.fill_points.append([self.now_x,self.now_y])
        return True

    def backward(self,distance):
        if(self.turtle):
            self.drawturtle(self.degree,self.backgroud_clr)
        temp_x=int(math.sin((self.degree+180)*math.pi/180)*distance)
        temp_y=int(math.cos((self.degree+180)*math.pi/180)*distance)

        self.now_x=temp_x+self.last_x
        self.now_y=temp_y+self.last_y

        if(self.pen):
            self.drawline()
        self.last_x=self.now_x
        self.last_y=self.now_y
        if(self.turtle):
            self.drawturtle(self.degree,self.tclr)
        if(self.beg_fill):
            self.fill_points.append([self.now_x,self.now_y])
        return True

    def pencolor(self,color):
        self.pclr=color
        return True

    def pensize(self,size):
        self.size=size
        return True

    def hideturtle(self):
        self.drawturtle(self.degree,self.backgroud_clr)
        self.turtle=False
        return True

    def showturtle(self):
        self.turtle=True
        self.drawturtle(self.degree,self.tclr)
        return True

    def position(self):
        print((self.now_x-120,self.now_y-120))
        return[self.now_x-120,self.now_y-120]

    def fillcolor(self,color):
        self.fill_clr=color
        return True

    def begin_fill(self):
        self.fill_points=[[self.now_x,self.now_y]]
        self.beg_fill=True
        return True

    def end_fill(self):
        if(self.fill_circle==False):
            
            Zx=0
            Zy=0
            for j in range (len(self.fill_points)):
                Zx=self.fill_points[j][0]+Zx
                Zy=self.fill_points[j][1]+Zy
            Zx=math.floor(Zx/len(self.fill_points))
            Zy=math.floor(Zy/len(self.fill_points))

            for i in range(len(self.fill_points)):
                if(i>0):
                    
                    q=np.sqrt((self.fill_points[i][0]-self.fill_points[i-1][0])**2+(self.fill_points[i][1]-self.fill_points[i-1][1])**2)
                    ux=(self.fill_points[i][0]-self.fill_points[i-1][0])/q
                    uy=(self.fill_points[i][1]-self.fill_points[i-1][1])/q
                    for l in np.arange(0,q,0.5):
                        px=self.fill_points[i-1][0]+l*ux
                        py=self.fill_points[i-1][1]+l*uy
                        self.drawline_direct_b(Zx, Zy, int(px), int(py), self.fill_clr,3)
                        
            self.fill_points=[]
            self.beg_fill=False
        else:
            if(self.fill_points[0][3]>=360):
                self.display.fill_circle(self.fill_points[0][0], self.fill_points[0][1], self.fill_points[0][2], self.fill_clr)
            else:
                Cx=self.fill_points[0][0]
                Cy=self.fill_points[0][1]
                R=self.fill_points[0][2]
                rad=self.fill_points[0][3]
                p1=0*math.pi/180
                p2=rad*math.pi/180
                dp=(p2-p1)/(8*R)

                for p in np.arange(p1,p2,dp):
                    px=Cx+R*math.sin(p)
                    py=Cy+R*math.cos(p)
                    self.drawline_direct_b(Cx, Cy, int(px), int(py), self.fill_clr,1)
            self.fill_points=[]
            self.beg_fill=False
        return True

    def bgcolor(self,color):
        self.backgroud_clr=color
        return True
    
    def circle(self,r,rad=360):
        print(rad)
        Cx=0
        Cy=0
        R=0
        if(r<0):
            temp_x=int(math.sin((self.degree+270)*math.pi/180)*(-r))
            temp_y=int(math.cos((self.degree+270)*math.pi/180)*(-r))
            for i in range (self.size):
                R=int(-r)-i
                if(R>0):
                    Cx=-self.now_x-temp_x
                    Cy=-self.now_y-temp_y
                    if(self.pen==True):
                        if(rad>=360):
                            self.display.circle(Cx, Cy, R, self.pclr)
                        else:
                            p1=0*math.pi/180
                            p2=rad*math.pi/180
                            dp=(p2-p1)/(8*R)

                            for i in range(self.size):
                                R=R-1
                                for p in np.arange(p1,p2,dp):
                                    px=Cx+R*math.sin(p)
                                    py=Cy+R*math.cos(p)
                                    self.display.pixel(int(px), int(py), self.pclr)
                R=int(-r)-i
        else:
            temp_x=int(math.sin((self.degree+270)*math.pi/180)*(r))
            temp_y=int(math.cos((self.degree+270)*math.pi/180)*(r))
            for i in range (self.size):
                R=int(r)-i
                if(R>0):
                    Cx=self.now_x+temp_x
                    Cy=self.now_y+temp_y
                    if(self.pen==True):
                        if(rad>=360):
                            self.display.circle(Cx, Cy, R, self.pclr)
                        else:
                            p1=0*math.pi/180
                            p2=rad*math.pi/180
                            dp=(p2-p1)/(8*R)

                            for i in range(self.size):
                                R=R-1
                                for p in np.arange(p1,p2,dp):
                                    px=Cx+R*math.sin(p)
                                    py=Cy+R*math.cos(p)
                                    self.display.pixel(int(px), int(py), self.pclr)
                R=int(r)-i

        if(self.beg_fill==True):
            self.fill_circle=True
            self.fill_points=[[Cx,Cy,R,rad]]
        return True

    def mode(self,m):
        if(m=="standard"):
            self.degree=90
            self.m="standard"
        if(m=="logo"):
            self.degree=0
            self.m="logo"
        return True
